#include <cmath>
#include <cstdio>
#include <iostream>

#include "ceres/ceres.h"
#include "ceres/rotation.h"

struct SnavelyReprojectionError {
  SnavelyReprojectionError(double observed_x, double observed_y, double *p3d)
      : observed_x(observed_x), observed_y(observed_y) {
      observedP[0] = p3d[0];
      observedP[1] = p3d[1];
      observedP[2] = p3d[2];
  }

  template <typename T>
  bool operator()(const T* const camera,
                  T* residuals) const {
    // camera[0,1,2] are the angle-axis rotation.
    T p[3];
    T *points = (T*)observedP;
    ceres::AngleAxisRotatePoint(camera, points, p);

    // camera[3,4,5] are the translation.
    p[0] += camera[3];
    p[1] += camera[4];
    p[2] += camera[5];

    // Compute the center of distortion. The sign change comes from
    // the camera model that Noah Snavely's Bundler assumes, whereby
    // the camera coordinate system has a negative z axis.
    T xp = p[0] / p[2];
    T yp = p[1] / p[2];

    // u,v
    const T& u = camera[7];
    const T& v = camera[8];
    // Compute final projected point position.
    const T& focal = camera[6];

    T predicted_x = focal * xp + u;
    T predicted_y = focal * yp + v;

    // The error is the difference between the predicted and observed position.
    residuals[0] = predicted_x - observed_x;
    residuals[1] = predicted_y - observed_y;

    return true;
  }

  // Factory to hide the construction of the CostFunction object from
  // the client code.
  static ceres::CostFunction* Create(const double observed_x,
                                     const double observed_y, double *p3D) {
    return (new ceres::AutoDiffCostFunction<SnavelyReprojectionError, 2, 9>(
        new SnavelyReprojectionError(observed_x, observed_y, p3D)));
  }

  double observed_x;
  double observed_y;
  double observedP[3];
};


struct DirectReprojectionError {
  DirectReprojectionError(double observed_x, double observed_y, double *p3d)
      : observed_x(observed_x), observed_y(observed_y) {
      observedP[0] = p3d[0];
      observedP[1] = p3d[1];
      observedP[2] = p3d[2];
  }

  template <typename T>
  bool operator()(const T* const l1, const T* const l2, const T* const l3, const T* const l4,
                  const T* const l5, const T* const l6, const T* const l7, const T* const l8,
                  const T* const l9, const T* const l10, const T* const l11, const T* const l12,
                  T* residuals) const {
    // camera[0,1,2] are the angle-axis rotation.
    T p[3];
    const double *obP = observedP;

    // camera[3,4,5] are the translation.
    p[0] += l1[0]*obP[0] + l2[0]*obP[1]  + l3[0]*obP[2]  + l4[0];
    p[1] += l5[0]*obP[0] + l6[0]*obP[1]  + l7[0]*obP[2]  + l8[0];
    p[2] += l9[0]*obP[0] + l10[0]*obP[1]  +l11[0]*obP[2]  + l12[0];

    // Compute the center of distortion. The sign change comes from
    // the camera model that Noah Snavely's Bundler assumes, whereby
    // the camera coordinate system has a negative z axis.
    T xp = p[0] / p[2];
    T yp = p[1] / p[2];

    T predicted_x = xp;
    T predicted_y = yp;

    // The error is the difference between the predicted and observed position.
    residuals[0] = predicted_x - observed_x;
    residuals[1] = predicted_y - observed_y;

    std::cout << "residuals:" << residuals[0] << ", " << residuals[1] << std::endl;

    return true;
  }

  // Factory to hide the construction of the CostFunction object from
  // the client code.
  static ceres::CostFunction* Create(const double observed_x,
                                     const double observed_y, double *p3D) {
    return (new ceres::AutoDiffCostFunction<DirectReprojectionError, 2, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1>(
        new DirectReprojectionError(observed_x, observed_y, p3D)));
  }

  double observed_x;
  double observed_y;
  double observedP[3];
};

int main(int argc, char *argv[])
{
    google::InitGoogleLogging(argv[0]);

    /**
    vector<Eigen::Vector3d> ptsRuler2DIronBallTop_W = {
        {33.50, 73.77, 78},
        {-3.27, 37, 78},
        {33.50, 0.23, 78},
        {70.27, 37, 78},
        {33.50, 63.77, 78},
    };
    vector<Eigen::Vector3d> ptsRuler2DIronBallBot_W = {
        {67, 7, 2},
        {0, 0, 2},
        {0, 74, 2},
        {67, 67, 2},
        {71.95, 2.05, 2},
        {55.06, 5.75, 2},
    };
    inner: 558.791 ,1481.58
    inner: 175.822 ,901.062
    inner: 754.578 ,534.524
    inner: 1130.44 ,1109.1
    inner: 585.582 ,1352.91
    outer: 1317.02 ,573.621
    outer: 386.046 ,264.715
    outer: 166.893 ,1322.2
    outer: 1147.49 ,1423.43
    outer: 1401.28 ,520.14
    outer: 1152.33 ,519.293
    **/

    double points[] = {
        33.50, 73.77, 78,
        -3.27, 37, 78,
        33.50, 0.23, 78,
        70.27, 37, 78,
        33.50, 63.77, 78,
        67, 7, 2,
        0, 0, 2,
        0, 74, 2,
        67, 67, 2,
        //71.95, 2.05, 2,
        //55.06, 5.75, 2,
    };

    double observations[] = {
        558.791 ,1481.58,
        175.822 ,901.062,
        754.578 ,534.524,
        1130.44 ,1109.1,
        585.582 ,1352.91,

        1317.02 ,573.621,
        386.046 ,264.715,
        166.893 ,1322.2,
        1147.49 ,1423.43,
        //1401.28 ,520.14,
        //1152.33 ,519.293,
    };
    double observations2[] = {
        1290.68 ,1513.81,
        783.605 ,1045.01,
        1254.05 ,542.663,
        1757.41 ,1011.03,
        1286.35 ,1381.44,
        1782.11 ,544.629,
        801.412 ,479.553,
        845.876 ,1559.89,
        1818.47 ,1418.17,
        1851.84 ,470.476,
        1607.66 ,532.807,
    };

    int numobservation = sizeof(observations)/sizeof (double);

#if 0
    // 旋转向量
    double camparas[9] = {1.785398, 0, 0,
                          0, 0, 300,
                         5000, 1032, 1032};

    // Create residuals for each observation in the bundle adjustment problem. The
    // parameters for cameras and points are added automatically.
    ceres::Problem problem;
    for (int i = 0; i < numobservation; ++i) {
      // Each Residual block takes a point and a camera as input and outputs a 2
      // dimensional residual. Internally, the cost function stores the observed
      // image location and compares the reprojection against the observation.

      ceres::CostFunction* cost_function = SnavelyReprojectionError::Create(
          observations[2 * i + 0], observations[2 * i + 1], points+i*3);
      problem.AddResidualBlock(cost_function,
                               NULL /* squared loss */,
                               camparas
                               );
    }

#endif
    // 旋转向量
    double camparas[12] = {309.575,	-64.2874,	-9.64305,	 8307.35,
                          69.9729,	 303.058,	  60.471,	  5548.6,
                         0.00382451,	-0.00250808,	0.0288485,	 21.3961};
//    double camparas[12] = {309.575,	-64.2874,	-9.64305,	 8307.35,
//                          69.9729,	 303.058,	  60.471,	  5548.6,
//                         1,	1,	1,	 1};

    // Create residuals for each observation in the bundle adjustment problem. The
    // parameters for cameras and points are added automatically.
    ceres::Problem problem;
    for (int i = 0; i < numobservation; ++i) {
      // Each Residual block takes a point and a camera as input and outputs a 2
      // dimensional residual. Internally, the cost function stores the observed
      // image location and compares the reprojection against the observation.

      ceres::CostFunction* cost_function = DirectReprojectionError::Create(
          observations[2 * i + 0], observations[2 * i + 1], points+i*3);
      problem.AddResidualBlock(cost_function,
                               NULL /* squared loss */,
                               &camparas[0], &camparas[1], &camparas[2], &camparas[3],
              &camparas[4], &camparas[5], &camparas[6], &camparas[7],
              &camparas[8], &camparas[9], &camparas[10], &camparas[11]
                               );
    }



    // Make Ceres automatically detect the bundle structure. Note that the
    // standard solver, SPARSE_NORMAL_CHOLESKY, also works fine but it is slower
    // for standard bundle adjustment problems.
    ceres::Solver::Options options;
    options.max_num_iterations = 100;
    options.gradient_tolerance = 1e-16;
    options.function_tolerance = 1e-16;
    options.linear_solver_type = ceres::DENSE_QR;
    options.minimizer_progress_to_stdout = true;

    ceres::Solver::Summary summary;
    ceres::Solve(options, &problem, &summary);
    std::cout << summary.FullReport() << "\n";

    std::cout << "campars: ";
    for (uint i=0; i<sizeof(camparas)/sizeof (double); i++) {
        std::cout << camparas[i] << ", ";
    }
    std::cout << std::endl;
    return 0;
}
